version = "v1.2.0"
from machine import SPI
from machine import I2C
from fpioa_manager import fm
from Maix import GPIO
import time
import utime
import sensor
import image
import struct
from machine import WDT
from machine import Timer
import audio
from Maix import GPIO, I2S
import uos
import os
from machine import UART
import KPU as kpu
from machine import WDT
import math

############################################数据定义#################################################
Line_P = 0.00511

class robot:
    def __init__(self):
        self.Mode = 0x00
        self.Action_sta = 0x00
        self.Action_id = 0x00
        self.X_speed = 0.00
        self.Y_speed = 0.00
        self.Z_speed = 0.00
        self.X_angle = 0.00
        self.Y_angle = 0.00
        self.Z_angle = 0.00
        self.High = 0x00
        self.slave_id = 0x00
        self.SpiData_R = bytearray(100)
        self.Joint_angle = [[0] * 3 for _ in range(4)] # 修改关节角度
        self.Initial_angle =  [[0] * 3 for _ in range(4)]# 初始角度
        self.Previous_angle = [[0] * 3 for _ in range(4)] # 存储上一步角度
        self.MachineState = 0 #反馈模式
        self.ActionState = 0 #反馈动作ID
        self.Img_ID = 0 #反馈摄像头ID
        self.Function_ID = 0#反馈功能
        self.RGB_Price = bytearray(4)
        #定时器
        self.led_sta = 0 #指示灯
        self.timing = 0
        #spi
        self.mosi=23 #4 23
        self.miso=22 #3 22
        self.cs=24   #5 24
        self.clk=21  #2 21
        self.signal = 9 #7 15
        #引脚映射
        fm.register(self.cs,fm.fpioa.GPIOHS10)#cs
        self.cs = GPIO(GPIO.GPIOHS10, GPIO.OUT)
        #握手信号引脚映射
        fm.register(self.signal,fm.fpioa.GPIOHS9)#11
        self.RedPIn = GPIO(GPIO.GPIOHS9,GPIO.IN)
        #其他引脚定义
        fm.register(12,fm.fpioa.GPIO0)#LED引脚初始化
        self.led=GPIO(GPIO.GPIO0,GPIO.OUT)
        fm.register(8,fm.fpioa.GPIO2)
        self.ESP32=GPIO(GPIO.GPIO2,GPIO.IN,GPIO.PULL_DOWN )


    ##############################################串口###############################################
    def uart_init(self):#串口初始化

        Uart_Tx = 6
        Uart_Rx = 7
        fm.register(Uart_Tx, fm.fpioa.UART1_RX, force=True)
        fm.register(Uart_Rx, fm.fpioa.UART1_TX, force=True)
        self.uart1 = UART(UART.UART1, 115200, 8, 0, 0, timeout=1000, read_buf_len=4096)

    def uart_rx(self):#串口读取
        global rx_num_now
        rx_num_now = 7
        sums = 0
        data_read = self.uart1.read()
        #print(data_read)
        if data_read:
            if(data_read[0] == 0xBA and data_read[1] == 0xBF ):
                lens = data_read[2]
                if(len(data_read)< lens):
                    return
                for i in range(0,lens):
                    sums += data_read[i]
                sums = sums&0xff #取低位
                if(sums == data_read[lens]):   # 求和校验
                    #print("校验成功")
                    self.MachineState = data_read[3]
                    self.ActionState = data_read[4]
                    self.Img_ID = data_read[5]
                    self.Function_ID = data_read[6]
                    self.Initial_angle[0][0] = self.decode_float(data_read,rx_num_now) # 大 左前
                    self.Initial_angle[0][1] = self.decode_float(data_read,rx_num_now) # 小
                    self.Initial_angle[0][2] = self.decode_float(data_read,rx_num_now) # 侧
                    self.Initial_angle[1][0] = self.decode_float(data_read,rx_num_now) # 大面 右前
                    self.Initial_angle[1][1] = self.decode_float(data_read,rx_num_now) # 小
                    self.Initial_angle[1][2] = self.decode_float(data_read,rx_num_now) # 侧面
                    self.Initial_angle[2][0] = self.decode_float(data_read,rx_num_now) # 大面 左后
                    self.Initial_angle[2][1] = self.decode_float(data_read,rx_num_now) # 小
                    self.Initial_angle[2][2] = self.decode_float(data_read,rx_num_now) # 侧面
                    self.Initial_angle[3][0] = self.decode_float(data_read,rx_num_now) # 大面 右后
                    self.Initial_angle[3][1] = self.decode_float(data_read,rx_num_now) # 小
                    self.Initial_angle[3][2] = self.decode_float(data_read,rx_num_now) # 侧面
                    #print(data_read)

    ###控制数据发送###uart方式
    def sport_control_send(self,Id):
        BYTE0 = lambda x : (x>>0)&0xff
        sum = 0x00
        len_data = 0
        data_to_send  =  []
        data_to_send.append(0xAA)
        data_to_send.append(0xAF)
        data_to_send.append(Id)  # 0x93:基础控制、0x92:角度
        data_to_send.append(0x00) #长度
        if (Id == 0x93):
            self.send_char(data_to_send,self.slave_id)#群控ID
            self.send_char(data_to_send,self.Mode)#Mode
            self.send_char(data_to_send,self.Action_sta)#Action_sta
            self.send_char(data_to_send,self.Action_id)#Action_id
            self.send_float(data_to_send,self.X_speed)#Robot_X_speed
            self.send_float(data_to_send,self.Y_speed)#Robot_Y_speed
            self.send_float(data_to_send,self.Z_speed)
            self.send_float(data_to_send,self.X_angle)
            self.send_float(data_to_send,self.Y_angle)
            self.send_float(data_to_send,self.Z_angle)
            self.send_char(data_to_send,self.High)#High
            len_data = len(data_to_send)
            data_to_send[3] = len_data -4 #有效数据长度
            for i in range(0,len_data):
                sum = sum +  data_to_send[i]
            data_to_send.append(BYTE0(int(sum))) #求和校验
        elif(Id == 0x92):
             self.send_char(data_to_send,self.slave_id)#群控ID
             self.send_char(data_to_send,0x01)#Mode
             self.send_float(data_to_send,self.Joint_angle[0][0]) # 大
             self.send_float(data_to_send,self.Joint_angle[0][1]) # 小
             self.send_float(data_to_send,self.Joint_angle[0][2]) # 侧
             self.send_float(data_to_send,self.Joint_angle[1][0]) # 大
             self.send_float(data_to_send,self.Joint_angle[1][1]) # 小
             self.send_float(data_to_send,self.Joint_angle[1][2]) # 侧
             self.send_float(data_to_send,self.Joint_angle[2][0]) # 大
             self.send_float(data_to_send,self.Joint_angle[2][1]) # 小
             self.send_float(data_to_send,self.Joint_angle[2][2]) # 侧
             self.send_float(data_to_send,self.Joint_angle[3][0]) # 大
             self.send_float(data_to_send,self.Joint_angle[3][1]) # 小
             self.send_float(data_to_send,self.Joint_angle[3][2]) # 侧
             len_data = len(data_to_send)
             data_to_send[3] = len_data -4 #有效数据长度
             for i in range(0,len_data):
                 sum = sum +  data_to_send[i]
             data_to_send.append(BYTE0(int(sum)))#求和校验
        elif(Id == 0x99):#RGB灯控制
            self.send_char(data_to_send,self.RGB_Price[0])
            self.send_char(data_to_send,self.RGB_Price[1])
            self.send_char(data_to_send,self.RGB_Price[2])
            self.send_char(data_to_send,self.RGB_Price[3])
            self.send_char(data_to_send,0x00)
            self.send_char(data_to_send,0x00)
            len_data = len(data_to_send)
            data_to_send[3] = len_data -4 #有效数据长度
            for i in range(0,len_data):
                sum = sum +  data_to_send[i]
            data_to_send.append(BYTE0(int(sum)))#求和校验

        tx_data =  bytearray(len_data+1)
        for i in range(0,len_data+1):
            tx_data[i] = data_to_send[i]
        self.uart1.write(tx_data)
        time.sleep_us(10)


    ##############################################数据转换############################################
    def send_float(self,tx_Buf,data):#float转4字节十六进制
        temp_B= struct.pack('f',float(data))
        tx_Buf.append(temp_B[0])
        tx_Buf.append(temp_B[1])
        tx_Buf.append(temp_B[2])
        tx_Buf.append(temp_B[3])

    def send_int(self,tx_Buf,data):#int转4字节十六进制
        temp_B= struct.pack('i',int(data))
        tx_Buf.append(temp_B[0])
        tx_Buf.append(temp_B[1])
        tx_Buf.append(temp_B[2])
        tx_Buf.append(temp_B[3])

    def send_char(self,tx_Buf,data):#char转4字节十六进制
        tx_Buf.append(int(data))

    def decode_float(self,rx_Buf,start_Byte_num):#4字节十六进制转char
        global rx_num_now
        temp=bytes([rx_Buf[start_Byte_num],rx_Buf[start_Byte_num+1],rx_Buf[start_Byte_num+2],rx_Buf[start_Byte_num+3]])
        rx_num_now= rx_num_now+4
        return struct.unpack('f',temp)[0]

    ############################################SPI通信##############################################
    def SPI_init(self):
        global spi1
        #开启SPI
        fm.register(self.mosi,fm.fpioa.SPI1_D0,force=True)#mosi
        fm.register(self.miso,fm.fpioa.SPI1_D1,force=True)#miso
        fm.register(self.clk,fm.fpioa.SPI1_SCLK,force=True)#sclk
        spi1 = SPI(SPI.SPI1, mode=SPI.MODE_MASTER, baudrate=4000000, polarity=0, phase=0, bits=8, firstbit=SPI.MSB)#10000000
        self.cs.value(1)
        time.sleep(1)
        #发送空数值
        data1 = bytearray(8)
        data1[0] = 0xAA
        data1[1] = 0xAF
        self.spi_send(data1)
        self.spi_send(data1)


    def Delay_H(self):###SPI等待握手###
        global spi1
        i=0
        while(1):
            self.cs.value(0)
            if self.RedPIn.value() == 1:
                return 1
            else:           #长时间失败退出
                i = i+1
                if i == 2000:
                    return 0

    def Delay_L(self):###spi等待握手###
        global spi1
        i=0
        while(1):
            self.cs.value(1)
            if self.RedPIn.value() == 0:
                return 1
            else:       #长时间失败退出
                i = i+1
                if i == 2000:
                    return 0

    def spi_send(self,data):###spi发送字节-（无接收数据）###
        if  len(data) == 0:
            return
        self.cs.value(0)
        if self.Delay_H():
            spi1.write(data)
        #else:
            #return
        self.cs.value(1)
        self.Delay_L()

    def spi_send_re(self,data):###spi发送字节+收数据###
        if  len(data) == 0:
            return
        self.cs.value(0)
        if self.Delay_H():
            spi1.write_readinto(data,self.SpiData_R)
        #else:
            #return
            #print(self.SpiData_R)
            self.spi_re_data(self.SpiData_R)
        self.cs.value(1)
        self.Delay_L()

    def spi_re_data(self,data_read):
        global rx_num_now
        rx_num_now = 7
        sums = 0
        if(data_read[0] == 0xBA and data_read[1] == 0xBF ):
            lens = data_read[2]
            if(len(data_read)< lens):
                return
            for i in range(0,lens):
                sums += data_read[i]
            sums = sums&0xff #取低位
            if(sums == data_read[lens]):   # 求和校验
                #print("校验成功")
                self.MachineState = data_read[3]
                self.ActionState = data_read[4]
                self.Img_ID = data_read[5]
                self.Function_ID = data_read[6]
                self.Initial_angle[0][0] = self.decode_float(data_read,rx_num_now) # 大 左前
                self.Initial_angle[0][1] = self.decode_float(data_read,rx_num_now) # 小
                self.Initial_angle[0][2] = self.decode_float(data_read,rx_num_now) # 侧
                self.Initial_angle[1][0] = self.decode_float(data_read,rx_num_now) # 大面 右前
                self.Initial_angle[1][1] = self.decode_float(data_read,rx_num_now) # 小
                self.Initial_angle[1][2] = self.decode_float(data_read,rx_num_now) # 侧面
                self.Initial_angle[2][0] = self.decode_float(data_read,rx_num_now) # 大面 左后
                self.Initial_angle[2][1] = self.decode_float(data_read,rx_num_now) # 小
                self.Initial_angle[2][2] = self.decode_float(data_read,rx_num_now) # 侧面
                self.Initial_angle[3][0] = self.decode_float(data_read,rx_num_now) # 大面 右后
                self.Initial_angle[3][1] = self.decode_float(data_read,rx_num_now) # 小
                self.Initial_angle[3][2] = self.decode_float(data_read,rx_num_now) # 侧面


    ############################################定时器###############################################
    def on_timer(self,timer):#LED灯闪烁
        data =  bytearray(100)
        self.led_sta = ~self.led_sta
        Robot.led.value(self.led_sta)
        Robot.uart_rx() #串口接收
        Robot.spi_send_re(data)


    def on_timer1(self,timer):
        global Robot,angles,times
        z = 0
        if(self.timing <= times):
            for i in range(0,4):
                for j in range(0,3):
                    self.Previous_angle[i][j] = self.Previous_angle[i][j] + angles[z]
                    self.Joint_angle[i][j] = self.Previous_angle[i][j]
                    z = z + 1
            self.sport_control_send(0x92)
            self.timing = self.timing + 1

    def Action_run(self):
        global Robot,angles,times
        while(self.timing <= times):
            z = 0
            for i in range(0,4):
                for j in range(0,3):
                    self.Previous_angle[i][j] = self.Previous_angle[i][j] + angles[z]
                    self.Joint_angle[i][j] = self.Previous_angle[i][j]
                    z = z + 1
            self.sport_control_send(0x92)
            self.timing = self.timing + 1
            time.sleep_ms(10)


    ############################################音频#################################################
    def calculate_angle(self,x1, y1, x2, y2):
        v1 = (x2 - x1, y2 - y1)
        v2 = (abs(v1[0]),abs(v1[1]))
        angle = math.tan(v2[0] / v2[1])
        if x1 < x2:
            angle = angle*-1
        angle_degrees = angle * (180 / math.pi)
        return angle_degrees

    ############################################图像识别#################################################

    def find_max(self,blobs):#寻找最大色块函数定义
        if blobs:
            max_size=0
            for blob in blobs:
                if blob[4] > max_size:
                    max_blob=blob
                    max_size = blob[4]
            return max_blob
############################################图形编程接口#################################################
def on_wdt(self):
    #print(self.context(), self)
    self.feed()
###########################基础控制
def Init():#初始化
    global mosi,miso,cs,clk,signal,RedPIn,spi1,led,ESP32,Robot,rx_num_now
    print(version)
    Robot = robot()
    rx_num_now = 3
    #开启串口
    Robot.uart_init()
    while(1):
        print("等待与主控连接")
        time.sleep(1)
        if Robot.ESP32.value() == 1:
            print("开启spi初始化")
            break
    wdt1 = WDT(id=1, timeout=1000, callback=on_wdt, context={})
    #开启定时器
    tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PERIODIC, period=1, unit=Timer.UNIT_S, callback=Robot.on_timer, arg=Robot.on_timer, start= False, priority=1, div=0)
    Robot.SPI_init()
    tim.start()#定时器启动
    print("启动成功")



def Mode(mode):#机器人模式 Id
    global Robot
    Robot.Mode = mode
    Robot.slave_id = 0 #Id
    Robot.sport_control_send(0x93)
    Robot.Mode = 0x00

def Action(ids,sta):#机器人动作组
    global Robot
    Robot.Action_sta = sta
    Robot.Action_id = ids
    Robot.slave_id = 0 #Id
    Robot.sport_control_send(0x93)
    time.sleep(1)
    Robot.Action_sta = 0x02
    Robot.Action_id = 0
    Robot.slave_id = 0 #Id
    Robot.sport_control_send(0x93)


def Xspeed(speed):#前进速度（-1.4~1.4）
    global Robot
    Robot.X_speed = speed
    Robot.slave_id = 0 #Id
    Robot.sport_control_send(0x93)

def Yspeed(speed):#左右移动速度（-1.4~1.4）
    global Robot
    Robot.Y_speed = speed
    Robot.slave_id = 0 #Id
    Robot.sport_control_send(0x93)

def Zspeed(speed):#右转转动速度（-1.4~1.4）
    global Robot
    Robot.Z_speed = speed
    Robot.slave_id = 0 #Id
    Robot.sport_control_send(0x93)

def Xangle(angle):#俯仰角度（-1.4~1.4）
    global Robot
    Robot.X_angle = angle
    Robot.slave_id = 0 #Id
    Robot.sport_control_send(0x93)

def Yangle(angle):#左右倾斜角度（-1.4~1.4）
    global Robot
    Robot.Y_angle = angle
    Robot.slave_id = 0 #Id
    Robot.sport_control_send(0x93)

def Zangle(angle):#左右扭动角度（-1.4~1.4）
    global Robot
    Robot.Z_angle = angle
    Robot.slave_id = 0 #Id
    Robot.sport_control_send(0x93)

def High(high):#高度（0~10）
    global Robot
    Robot.High = high
    Robot.slave_id = 0 #Id
    Robot.sport_control_send(0x93)

def Reset():#复位
    Robot.X_speed = 0
    Robot.Y_speed = 0
    Robot.Z_speed = 0
    Robot.X_angle = 0
    Robot.Y_angle = 0
    Robot.Z_angle = 0
    Robot.slave_id = 0 #Id
    Robot.sport_control_send(0x93)

def Sta(): ##接收数据-机器人状态、k210控制功能数据（机器人状态、动作组状态、摄像头控制1、功能控制2）
    global Robot
    data_read = []
    data_read.append(Robot.MachineState)
    data_read.append(Robot.ActionState)
    data_read.append(Robot.Img_ID)
    data_read.append(Robot.Function_ID)
    return data_read

def Status(): ##机器人状态
    global Robot
    data_read = []
    data_read.append(Robot.MachineState)
    return data_read

def Action_Status(): ##动作组状态
    global Robot
    data_read = []
    data_read.append(Robot.ActionState)
    return data_read

###########################图传
def img_init(): #图像初始化
    global image_type
    sensor.binocular_reset()
    sensor.shutdown(0)  # 选中sensor 0
    sensor.set_pixformat(sensor.RGB565)
    sensor.set_framesize(sensor.QVGA) #QVGA
    sensor.set_vflip(1)

    sensor.shutdown(1)  # 选中sensor 1
    sensor.set_pixformat(sensor.RGB565)
    sensor.set_framesize(sensor.QVGA) #QVGA
    sensor.run(1)

    sensor.shutdown(0)  # 开启前置摄像头
    images = sensor.snapshot()
    image_type = type(images)

def img_app(imgs,size):#图传
    global image_type,Robot
    fm.register(Robot.mosi,fm.fpioa.SPI1_D0,force=True)#mosi
    fm.register(Robot.miso,fm.fpioa.SPI1_D1,force=True)#miso
    fm.register(Robot.clk,fm.fpioa.SPI1_SCLK,force=True)#sclk 10000000 4000000
    spi1 = SPI(SPI.SPI1, mode=SPI.MODE_MASTER, baudrate=10000000 , polarity=0, phase=0, bits=8, firstbit=SPI.MSB)
    if imgs == None:
        return
    #if type(imgs) !=  image_type:
        #return

    imgs = imgs.compressed(quality=size)
    img_byte = imgs.to_bytes(1,'little')
    img_len = int(len(img_byte))
    #Function(0x0b,img_len) #发送图传指令
    Function(4,1024) #设置图传数据1024/段
    block = int(img_len/1024)
    block1 = img_len- block*1024
    for i in range(0,block):
        Robot.spi_send(img_byte[i*1024:(i+1)*1024])
    if(block1>0):
        datas = bytearray(4)
        datas[0] = 0x00
        datas[1] = 0x00
        datas[2] = 0x00
        datas[3] = 0x00
        Function(4,block1)
        if(block1<1020):
            img_byte = img_byte[block*1024:] + datas
            Robot.spi_send(img_byte)
        else:
            Robot.spi_send(img_byte[block*1024:])
    Function(0x0b,img_len) #发送图传指令

############################################图像功能
def IdentifyLine(img,threshold):#小球识别
    pixels= 100 #识别小球最小像素
    blobs = img.find_blobs([threshold],x_stride=20,y_stride=20,merge=True)
    if blobs:
        for blob in blobs:
            #if b.pixels() > pixels :
            if blob.density() > 0.45 and blob.pixels() > 500:
                #img.draw_rectangle(b[0:4])
                img.draw_circle((blob.cx(),blob.cy(),int((blob.w()+blob.h())/4)))
                #img.draw_cross(b[5], b[6])
                x = 160 - blob.cx()
                y = 120 - blob.cy()#2/27修改
                Ball_data = []
                Ball_data.append(x)
                Ball_data.append(y)
                Ball_data.append(blob.pixels())
                #print(y)
                return Ball_data
    return None

num = 0
Y = 0
xx = 0
def Ball_Tracking(X):#小球跟随
    global Robot,num,Y,xx
    Robot.High = 10
    if X != None: #if X != None and X[1] >200
        num = 0
        if X[2] >= 2500:### 近距离
            Robot.Mode = 7   #站立
            if Robot.MachineState == 1: #判断进入站立模式
                Y = Y + (X[1]*0.00311) ### Y轴位置
                if Y >= 1.4:   ### 限制 1.4机器人最高仰视位置
                    Y = 1.4
                if Y <= -1.4: ### 限制 -1.4机器人最低俯视位置
                    Y = -1.4
                Robot.X_angle = -Y
                xx = -(X[0]*0.00511) ### X轴位置
                if xx >= 1.4:   ### 限制 1.4机器人最高仰视位置
                    xx = 1.4
                if xx <= -1.4: ### 限制 -1.4机器人最低俯视位置
                    xx = -1.4
                Robot.Z_angle = -xx
            else:
                Robot.X_angle = 0
                Robot.Z_angle = 0
        elif X[2] < 2500 :
            Robot.Mode = 5
            if Robot.MachineState == 13:
                Robot.X_speed = 0.6
                Y = Y + (X[1]*0.00211) ### Y轴位置
                if Y >= 1.4:   ### 限制 1.4机器人最高仰视位置
                    Y = 1.4
                if Y <= -1.4: ### 限制 -1.4机器人最低俯视位置
                    Y = -1.4
                Robot.X_angle = 0
                xx = -(X[0]*0.00211) ### X轴位置
                Robot.Z_speed = xx
            else:
                Robot.X_speed = 0
                Robot.X_angle = 0
                Robot.Z_speed = 0

    else: ###长时间无识别到，停止
        num = num + 1
        if num == 5 :
            Robot.X_angle = 0
            Robot.X_speed = 0
            Robot.Z_speed = 0
            Robot.Mode = 7
    # 优化卡顿问题 2023/03/24
    Robot.sport_control_send(0x93)

def Line_distinguish(img,black_threshold):#线条识别
    global Robot
    xx = 0
    yy = 0
    x = 0
    y = 0
    x1 = 0
    y1 = 0
    s = 0
    for i in range(0,5):
        ii = i*24
        blobs = img.find_blobs([black_threshold], roi=[0,ii,320,ii+24],x_stride = 10,pixels_threshold = 10,merge_cb=None)#merge_cb=True
        if blobs:
            max_blob = Robot.find_max(blobs)
            xx = xx + max_blob[5]
            yy = yy + max_blob[6]
            s = s+1
    if s > 1:
        x = int(xx /s)
        y = int(yy /s)
        img.draw_cross(x, y)
    s =0
    xx = 0
    yy = 0
    for i in range(5,10):
        ii = i*24
        blobs = img.find_blobs([black_threshold], roi=[0,ii,320,ii+24],x_stride = 10,pixels_threshold = 10,merge_cb=None)#merge_cb=True
        if blobs:
            max_blob = Robot.find_max(blobs)
            xx = xx + max_blob[5]
            yy = yy + max_blob[6]
            s = s+1
    if s > 1:
        x1 = int(xx /s)
        y1 = int(yy /s)
        img.draw_cross(x1, y1)
    if x and x1:
        img.draw_line((x,y,x1,y1), color = 150)
        xs = (x+x1)/2
        xs = 160 - xs
        angles = Robot.calculate_angle(x,y,x1,y1)
        #print("角度=",angles)
        return int(xs),int(angles)
    return None

def Line_patrol(X):#巡线
    Robot.High = 10
    if X == None:
        Robot.X_speed = 0
        Robot.Z_speed = 0
        Robot.Mode = 7
        Robot.sport_control_send(0x93)
        return
    Robot.Mode = 5 #慢速对角步态
    Robot.X_speed = 0.6
    X = -(X[0]*Line_P) + (-(X[0]*0.00211))
    Robot.Z_speed = X
    Robot.sport_control_send(0x93)

def Line_patrols(*args):#巡线  //位置、角度、步态、速度、PosPID、anglePID
    Robot.High = 10
    if args[0] == None:
        Robot.X_speed = 0
        Robot.Z_speed = 0
        Robot.Mode = 7
        Robot.sport_control_send(0x93)
        return
    Robot.Mode = args[1] #慢速对角步态
    Robot.X_speed = (args[2] / 10)
    Zspeed = -(args[0][0]*args[3]) + (-(args[0][1]*args[4]))
    Robot.Z_speed = Zspeed
    Robot.sport_control_send(0x93)

#####################################动作编辑
def Action_On(): #开启动作编辑
    global Robot
    # 存储当前关节角度
    time.sleep(3)
    # 将动作前关节角度存储到上一步关节角度
    for i in range(0,4):
        for j in range(0,3):
            Robot.Previous_angle[i][j] = Robot.Initial_angle[i][j]

'''
    函数：关节角度控制
    leg:腿的位置 0:左前腿、1：右前腿、2：左后腿、3：右后腿
    joint:关节位置 0：大腿、1：小腿、3：侧腿
    angle:角度 （-90~0~90）
'''
def Joint_Control(leg,joint,angle):
    global Robot
    # 存储要执行的关节角度
    Robot.Joint_angle[leg][joint] = angle

'''
    函数：关节角度控制
    leg:腿的位置 0:左前腿、1：右前腿、2：左后腿、3：右后腿
    angle:角度 0：大腿、1：小腿、3：侧腿（-90~0~90）
'''
def Joint_Controls(leg,angle1,angle2,angle3):
    global Robot
    # 存储要执行的关节角度
    Robot.Joint_angle[leg][0] = angle1
    Robot.Joint_angle[leg][1] = angle2
    Robot.Joint_angle[leg][2] = angle3

### 动作执行
''' Time:步骤执行时间,单位：秒'''
def Action_Execution(Time):
    global Robot,angles,tim,times
    Robot.slave_id = 0 #Id
    times = Time * 100
    angles = []
    # 执行改变的关节角度，并耗时多少时间达到指定角度
    Robot.timing = 0
    for i in range(0,4):
        for j in range(0,3):
            angles.append(-(Robot.Previous_angle[i][j] - Robot.Joint_angle[i][j])/(Time*100))
    #tim = Timer(Timer.TIMER1, Timer.CHANNEL1, mode=Timer.MODE_PERIODIC, period=10, unit=Timer.UNIT_MS, callback=Robot.on_timer1,arg=Robot.on_timer1,start=False, priority=2, div=0)
    #tim.start() #开启定时器
    #time.sleep(Time+0.1)
    #tim.stop() #停止定时器
    Robot.Action_run()
    # 存储改变后的角度
    for i in range(0,4):
        for j in range(0,3):
            Robot.Previous_angle[i][j] = Robot.Joint_angle[i][j]

def Action_Off():# 关闭动作编辑
    global Robot,angles,tim,times
    # 恢复到开启动作编辑的前角度
    z = 0
    for i in range(0,4):
        for j in range(0,3):
            angles[z] = ((-(Robot.Previous_angle[i][j] - Robot.Initial_angle[i][j]))/(2*100))
            z = z + 1
    times = 200
    Robot.timing = 0
    #tim.start()
    #time.sleep(2)
    #tim.stop()
    #del tim
    Robot.Action_run()

#####################################语音-文字转音频
def Str_audio(data):###文字转语音###
    global Robot
    utf = data.encode('UTF-8')
    data =  bytearray(4)
    data[0] = 0xFF
    data[1] = 0xFF
    data[2] = 0xFF
    data[3] = 0xFF
    datas = utf + data
    Function(5,len(utf))
    Robot.spi_send(datas)
############################################手势识别#################################################
PAJ7620_ID = 0x73                   #从机ID
PAJ7620_REGITER_BANK_SEL = 0xEF     #选择寄存器带区
PAJ7620_BANK0 = 0x00                #带区0
PAJ7620_BANK1 = 0x01                #带区1

ISENABLE = 0x72                     #使能/失能寄存器
GES_DETECT = 0x43                   #读取手势（1-8）
GES_DETECT2 = 0x44                  #读取手势（9）
SUSPEND = 0x03                      #使模块挂起（休眠）

#手势识别方式
left = 1                            #从右到左
right = 2                           #从左到右
down = 4                            #从上往下
up = 8                              #从下往上
forward = 16                        #从后往前
backward = 32                       #从前往后
clockwise = 64                      #顺时针
count_clockwise = 128               #逆时针
wave = 1                            #挥手

Init_Register_Array = [
    [0xEF, 0x00],
    [0x37, 0x07],
    [0x38, 0x17],
    [0x39, 0x06],
    [0x41, 0x00],
    [0x42, 0x00],
    [0x46, 0x2D],
    [0x47, 0x0F],
    [0x48, 0x3C],
    [0x49, 0x00],
    [0x4A, 0x1E],
    [0x4C, 0x20],
    [0x51, 0x10],
    [0x5E, 0x10],
    [0x60, 0x27],
    [0x80, 0x42],
    [0x81, 0x44],
    [0x82, 0x04],
    [0x8B, 0x01],
    [0x90, 0x06],
    [0x95, 0x0A],
    [0x96, 0x0C],
    [0x97, 0x05],
    [0x9A, 0x14],
    [0x9C, 0x3F],
    [0xA5, 0x19],
    [0xCC, 0x19],
    [0xCD, 0x0B],
    [0xCE, 0x13],
    [0xCF, 0x64],
    [0xD0, 0x21],
    [0xEF, 0x01],
    [0x02, 0x0F],
    [0x03, 0x10],
    [0x04, 0x02],
    [0x25, 0x01],
    [0x27, 0x39],
    [0x28, 0x7F],
    [0x29, 0x08],
    [0x3E, 0xFF],
    [0x5E, 0x3D],
    [0x65, 0x96],
    [0x67, 0x97],
    [0x69, 0xCD],
    [0x6A, 0x01],
    [0x6D, 0x2C],
    [0x6E, 0x01],
    [0x72, 0x01],
    [0x73, 0x35],
    [0x74, 0x00],
    [0x77, 0x01]]

Init_PS_Array = [
    [0xEF, 0x00],
    [0x41, 0x00],
    [0x42, 0x00],
    [0x48, 0x3C],
    [0x49, 0x00],
    [0x51, 0x13],
    [0x83, 0x20],
    [0x84, 0x20],
    [0x85, 0x00],
    [0x86, 0x10],
    [0x87, 0x00],
    [0x88, 0x05],
    [0x89, 0x18],
    [0x8A, 0x10],
    [0x9f, 0xf8],
    [0x69, 0x96],
    [0x6A, 0x02],
    [0xEF, 0x01],
    [0x01, 0x1E],
    [0x02, 0x0F],
    [0x03, 0x10],
    [0x04, 0x02],
    [0x41, 0x50],
    [0x43, 0x34],
    [0x65, 0xCE],
    [0x66, 0x0B],
    [0x67, 0xCE],
    [0x68, 0x0B],
    [0x69, 0xE9],
    [0x6A, 0x05],
    [0x6B, 0x50],
    [0x6C, 0xC3],
    [0x6D, 0x50],
    [0x6E, 0xC3],
    [0x74, 0x05]]

Init_Gesture_Array = [
    [0xEF, 0x00],
    [0x41, 0x00],
    [0x42, 0x00],
    [0xEF, 0x00],
    [0x48, 0x3C],
    [0x49, 0x00],
    [0x51, 0x10],
    [0x83, 0x20],
    [0x9F, 0xF9],
    [0xEF, 0x01],
    [0x01, 0x1E],
    [0x02, 0x0F],
    [0x03, 0x10],
    [0x04, 0x02],
    [0x41, 0x40],
    [0x43, 0x30],
    [0x65, 0x96],
    [0x66, 0x00],
    [0x67, 0x97],
    [0x68, 0x01],
    [0x69, 0xCD],
    [0x6A, 0x01],
    [0x6B, 0xB0],
    [0x6C, 0x04],
    [0x6D, 0x2C],
    [0x6E, 0x01],
    [0x74, 0x00],
    [0xEF, 0x00],
    [0x41, 0xFF],
    [0x42, 0x01]]


#i2c初始化
i2c = I2C(I2C.I2C5, mode=I2C.MODE_MASTER, scl=30,sda=31,freq=100000, addr_size=7)

#iic设备获取
def IIC_Device():
   devices = i2c.scan()
   return devices


#选择寄存器带区
def GestureSelectBank(bank):
    if bank == 0 :
        i2c.writeto_mem(PAJ7620_ID, PAJ7620_REGITER_BANK_SEL, PAJ7620_BANK0, mem_size=8)#写数据到从机寄存器
    elif bank == 1 :
        i2c.writeto_mem(PAJ7620_ID, PAJ7620_REGITER_BANK_SEL, PAJ7620_BANK1, mem_size=8)#写数据到从机寄存器
    else :
        return 255


#发送带区地址和数据
def GestureWriteReg(addr, cmd):
    write_buf = bytearray(2)
    write_buf[0] = addr
    write_buf[1] = cmd
    i2c.writeto_mem(PAJ7620_ID, write_buf[0], write_buf[1], mem_size=8)

#读取手势
def GestureReadReg(addr):
    read_buf = bytearray(1)
    read_buf[0] = addr
    i2c.writeto(PAJ7620_ID, read_buf)

    result = bytearray(1)
    i2c.readfrom_mem_into(PAJ7620_ID, read_buf[0], result, mem_size=8)
    return result

#唤醒和发送数组
def GestureInit():
    #utime.sleep_ms(500)
    for i in range(0,5):
       wakeup = GestureReadReg(0x00)
       utime.sleep_ms(1)
    GestureSelectBank(0)
    result = bytearray(1)
    i2c.readfrom_mem_into(PAJ7620_ID, 0x00, result, mem_size=8)
    #print('result=',result[0])
    if result[0] != 0x20:
        return 0xff
    for i in range(0,len(Init_Register_Array)):
        GestureWriteReg(Init_Register_Array[i][0], Init_Register_Array[i][1])
    for i in range(0,len(Init_Gesture_Array)):
        GestureWriteReg(Init_Gesture_Array[i][0], Init_Gesture_Array[i][1])
    GestureSelectBank(1)#带区1
    GestureWriteReg(ISENABLE, 0x01)#使能
    return 1

#获取手势
def GetGesture():
    GestureSelectBank(0)
    date = GestureReadReg(0x43)
    if date != right or date != left or date != up or date != down  or date != forward or date != backward or date != clockwise : #or date != count
        date == GestureReadReg(0x44)
        if date == wave:
            return 256
        return date[0]


#选择手势
def Select_gesture_as(state):
        return state
############################################超声波#################################################
redata = bytearray(3)#返回数据
i2c5 = I2C(I2C.I2C5, mode=I2C.MODE_MASTER, scl=30,sda=31,freq=100000, addr_size=7)
uladdr = 0x57

def ultrasonic_init(uladdr):
    i2c5.writeto(uladdr, '\x01')
    utime.sleep_ms(100)#等待100ms
    i2c5.readfrom_into(0x57, redata)
    #print(redata)
    x = ((redata[0]<<16) + (redata[1]<<8) + redata[2])/1000
    #print("x=",x)
    return x

#i2c初始化,获取总线设备
def i2cscan_ultrasonic():
    i2c2 = I2C(I2C.I2C5, mode=I2C.MODE_MASTER, scl=30,sda=31,freq=100000, addr_size=7)#400000
    devices = i2c2.scan()
    #print(devices)
    if devices:
        for device in devices:
            ultrasonic_init(device)
            #print(ultrasonic_init(device))

def i2cscan_gesture():
    i2c2 = I2C(I2C.I2C5, mode=I2C.MODE_MASTER, scl=30,sda=31,freq=100000, addr_size=7)#400000
    devices = i2c2.scan()
    GestureInit()
    if GestureInit() == 1:
        return 1

###功能选择###
def Function(ii,lens):
    global Robot
    data =  bytearray(1024)
    data[0] = 0xEA
    data[1] = 0xEB #帧头
    data[2] = ii #功能帧
    data[3] = (lens >> 8)
    data[4] = (lens & 0x00FF)
    data[5] = 0xEE
    data[6] = 0xEF #帧尾
    data[7] = 0x01
    data[1023] = 0x00
    #print(data )
    Robot.spi_send(data)


